#ifndef __COMUTILITIES_H_INCLUDED__ 
#define __COMUTILITIES_H_INCLUDED__

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <Eigen/Core>

#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>

#include <map>

class CoMUtilities
{
public:
	// il poligono di supporto è da settare in base al sistema di riferimento
	double support_polygon_x; // -0.0355 <= x <= 0.0355
	double support_polygon_y; // -0.09 <= y <= 0.09

	pcl::visualization::PCLVisualizer *p;

	tf::TransformListener* listener;
	double robovie_x_mass;
	std::map< std::string, std::vector<double> > tfs;
	Eigen::Vector3f com;

	bool pcl_com_visualization;

	CoMUtilities();

	CoMUtilities(double my_support_polygon_x, double my_support_polygon_y, bool visualize_com_in_pcl);

	/* 
		Inizializza la mappa tfs tale che
		key = link name, value = mass and com x y z [meters],
		i pesi usati sono presi dal paper e la loro somma corrisponde a quanto riportato
		nelle specifiche di robovie_x,
	   	probabilmente i valori dell'urdf sono migliori perché maggiori di quelli del paper 
	   	che tra l'altro non	considerano il peso dei motori, della batteria e dei sensori
	*/
	void init_links_params();

	/*
		Inizializza la massa di robovie_x
	*/
	void init_robovie_x_mass();

	/*
		Visualizza a terminale l'ascissa del CoM
		rispetto le ascisse minime e massime del poligono di sostentamento,
		l'asse X è più significativo dell'Y perché il robot tende a sbilanciarsi
		in avanti o indietro
	*/
	void showComX();

	/*
		Visualizza i CoM dei link del robot sul visualizzatore di PCL
	*/
	void visualize_links_coms(pcl::PointCloud<pcl::PointXYZ>::Ptr coms);

	/*
		Visualizza il CoM del robot e la sua proiezione a terra sul visualizzatore di PCL
	*/
	void visualize_com(pcl::PointCloud<pcl::PointXYZ>::Ptr com_pcl);

	/*
		Verifica che il CoM sia nel poligono di sostentamento
	*/
	int is_com_in_support_polygon();

	/* 
	    tf from source to target
	    source will be our reference system
	    target will be our link
	    because we want the translation from the reference system to the link
	    i.e. let the torso be our reference system, torso_lrf is (0, 0, 0) while head_lrf is (0, 0, 0.3)
	    the tf from the torso to the head is (0, 0, 0.3) <- what I'm interested in
	    the tf from the head to the torso is (0, 0, -0.3)
	*/
	Eigen::Vector3f tf_source2target(const std::string & source_frame, const std::string & target_frame);

	/*
		Calcola il CoM
	*/
	void calculate_com();
};

#endif